/**
 * Prototype Camera Subsystem, meant to give us some manual 
 * control over certain aspects of the camera itself.
 * Currently, allocations have been made for the extraction
 * of the Luminance plane to find some bright areas of an
 * image. Other than that, no further "tracking" stuff has
 * been created. This should serve as an "eh" template for
 * future camera systems.
 */

package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.image.*;

/**
 * @author Jarrod Risley
 */

/*
 * Note: All functions check to see if isCameraActive is true or not. This is a 
 * simple way to keep the bot running in the event that the camera is not initialized
 * correctly.
 */


public class VisualTrackingSubsystem extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    /*
     * Global Declarations
     */
    
    AxisCamera camera;
    private ColorImage image;
    private MonoImage lumPlane;
    private BinaryImage bim;
    private CriteriaCollection coll = new CriteriaCollection();
    
    String address;
    String CameraName;
    private boolean isCameraActive = false;
    private int brightnessLevel;
    private int framesPerSecond;
    public int kColorLevel;
    private int luminanceValue;
    private boolean isLumPlaneActive = false;
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(null);
    } // end initDefaultCommand
    
    /**
     * Sets the brightness level for the camera.
     * 
     * @param level The brightness level, a number from 0 to 100.
     */
    
    public void setBrightness(int level) {
        if (isCameraActive) {
            level = brightnessLevel;
            camera.writeBrightness(brightnessLevel);
        } // end if
    } // end setBrightness
    
    // Automatically sets
    public void setWhiteBalance() {
        if (isCameraActive) {
            camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.automatic);
        } // end if
    } // end setWhiteBalance
    
    /**
     * Sets the amount of pictures to take in one second. Note that more frames 
     * means a larger load to process and may slow down the robot, but will lead
     * to better results.
     * 
     * @param fps The number of frames to take per second.
     */
    
    public void setFramesPerSecond(int fps) {
        if (isCameraActive) {
        fps = framesPerSecond;
        camera.writeMaxFPS(framesPerSecond);
        } // end if
    } // end setFramesPerSecond
    
    /**
     * Sets the resolution of the camera. Note that a higher resolution will 
     * lead to a higher processing time, but better results.
     */
    
    public void setResolution() {
        if (isCameraActive) {
        camera.writeResolution(AxisCamera.ResolutionT.k640x480); // could be declared globally?
        } // end if
    } // end setResolution
    
    /**
     * Updates the driverstation.
     */
    
    public void updateDashboard() {
        if (isCameraActive) {
        DriverStationLCD.getInstance().updateLCD();
        } // end if
    } // end updateDashboard
     
    /**
     * Gets the image from the camera.
     * 
     * @throws AxisCameraException
     * @throws NIVisionException 
     * @return A ColorImage object to manipulate.
     */
    
    public ColorImage getImage() {
        if (isCameraActive) {
            
            try {
            
                if (false == camera.freshImage()) {
                    image = camera.getImage();
                    DriverStationLCD.getInstance().updateLCD();
                    return image;
                } else {
                    kColorLevel = camera.getColorLevel();
                    image.replaceLuminancePlane(lumPlane);
                    isLumPlaneActive = true;
                    System.out.println(lumPlane);
                    image.free();
                } // end if-else
            }  catch (AxisCameraException ex) {} 
            catch (NIVisionException ex) {} // At some point, fill these

        } // end if
        
        return image;
        
    } // end getImage
    
    /**
     * @depreciated Extracts the Luminance Plane from the image. 
     * This method is obsolete.
     * 
     * @throws AxisCameraException
     * @throws NIVisionException 
     */
    
    public void findLuminance() throws AxisCameraException, NIVisionException {
        if ((isCameraActive) && (isLumPlaneActive)) {
            image.replaceLuminancePlane(lumPlane);
            System.out.println(lumPlane);
        } else if (false == isCameraActive) {
            System.out.println("Error: " + CameraName + " not active.");
        } else if (false == isLumPlaneActive) {
            System.out.println("Error: Luminance plane replacement failed"
                    + " --- lumPlane not extracted.");
        } else if ((false == isCameraActive) && (false == isLumPlaneActive)) {
            System.out.println("Error: " + CameraName + "not active and "
                    + "Luminance plane replacement failed --- "
                    + "lumPlane not extracted.");
        } // end rediculously large if-else statement
    } // end findLuminance
    
    /**
     * Thresholds the ColorImage.
     * 
     * @param im The ColorImage to convert.
     * @return A BinaryImage that has been convexHull'd.
     */
    
    public BinaryImage thresholdImage(ColorImage im) {
        
        if (isCameraActive) {
            try {
                // Tweak values to facilitate needs.
                BinaryImage b = im.thresholdRGB(215, 255, 215, 255, 215, 225);
                bim = b.convexHull(true);
            } catch (NIVisionException ex) {}

            return bim;
        } // end if
        
        return bim;
        
    } // end thresholdImage
    
    /**
     * Filters the BinaryImage and cleans out small particles.
     * 
     * @param bi The BinaryImage to pass in.
     * @return A BinaryImage that has been processed.
     */
    
    public BinaryImage filterParticles(BinaryImage bi) {
        
        if (isCameraActive) {
            try {
                BinaryImage carrier = bi.removeSmallObjects(true, 3);
                bim = carrier.particleFilter(coll); // need to set Criteria for this
            } catch (NIVisionException ex) {}

            return bim;
            
        } // end if
        
        return bim;
        
    } // end filterSmallParticles
    
    /**
     * Analyzes the BinaryImage passed in for targets.
     * @param bi A BinaryImage to be searched for particles of a particlar area.
     */
    
    public void analyze(BinaryImage bi) {
        try {
            
            double area;
            ParticleAnalysisReport pa[] = bi.getOrderedParticleAnalysisReports();
            
            for (int z = 0; z < pa.length; z++) {
               
               area = pa[z].particleArea;
               
               if (area >= 50 && area <= 100) {
                   System.out.println("Possible target found at X: " 
                           + pa[z].center_mass_x 
                           + " Y: " + pa[z].center_mass_y);
               } else {
                   System.out.println("Particle at index " + z + "is not a "
                           + "target.");
               } // end if-else
               
            } // end for
            
        } catch (NIVisionException ex) {}
        
    } // end analyze
    
    /**
     * Checks to see if the Camera Subsystem is online. It is strongly 
     * recommended that you include this check whenever you call upon the 
     * Camera Subsystem.
     * 
     * @return The state of the Camera; false is offline, true is online.
     */
    
    public boolean getCameraState() {
        
        return isCameraActive;
        
    } // end getCameraState
    
    /**************************************************************************
     *                               CONSTRUCTOR
     *************************************************************************/
    
    /**
     * Constructor for the Camera Subsystem.
     * 
     * @param name The name of the camera.
     * @param ip The IP address for the camera.
     */
    
    public VisualTrackingSubsystem(String name, String ip) {
        address = ip;
        CameraName = name;
        camera = AxisCamera.getInstance(); // Sets the IP adress.
        
        coll = new CriteriaCollection();
            coll.addCriteria(NIVision.MeasurementType.IMAQ_MT_AREA, 50, 
                    100, true);
        
        if (camera.equals(address)) {
            isCameraActive = true;
            System.out.println("VisualTrackingSubsystem " + CameraName 
                    + "created.");
        } else {
            isCameraActive = false;
            System.out.println("Error creating VisualTrackingSubsystem: " 
                    + CameraName
                    + ". ");
            System.out.println("-----CAMERA OFFLINE-----");
        } // end if-else
    } // end VisualTrackingSubsystem Constructor
    
/**
* ***************************WARNING************************************ 
*                THE FOLLOWING IS AN ABSOLUTE MUST:
* 
* Whenever you call forth an image, you need to make sure that after
* all operations have been completed on the image (i.e. extract 
* Luminance plane, find a square targe, and so forth), the image itself 
* MUST BE DELETED. Otherwise epic sad face will occur. We don't want
* epic sad face. So when you're done, call this function - this takes
* care of this for you, so you can go track your square :D
*/

    public void deleteImage(Image i) {
        
        try {
            i.free(); // Deletes the image. CALL THIS LAST.
        } catch (NIVisionException ex) {}
        
    } // end deleteImage
    
} // end VisualTrackingSubsystem